/**
  ******************************************************************************
  * @file    lite_dog.c
  * @author  GEEKROS,  site:www.geekros.com
  ******************************************************************************
*/

#include "lite_dog.h"
#include "dog_app.h"
#include "dog_move.h"
#include "dog_order.h"
#include "dog_remote.h"

Lite_Dog_Struct Lite_Dog;

/*******************************************************************************
  * @funtion      : Lite_Dog_Init_Motion_Set  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗初始化动作设置
  * @param         {Lite_Dog_Struct*} Dog_Init 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Init_Motion_Set(Lite_Dog_Struct* Dog_Init)
{
	
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Data_Init  
  * @LastEditors  : JackyJuu
  * @description  : 机器狗数据初始化
  * @param         {Lite_Dog_Struct*} Dog_Init 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Data_Init(Lite_Dog_Struct* Dog_Init)
{
	// // 打开蓝色LED灯
    // Led_State("blue", "on");

	//打开扩展板供电电源接口
	Power_24V_State(0, "on");
	
	//设置舵机串口波特率
	Servo_Init(1000000);
	
	//设置舵机类型
	Servo_Type(0, SERVO_SCS);
	
	//设置电池低电压报警阀值，低电量时开发板红色LED灯亮起
	Adc_Low_Voltage(11.4);

	// 初始模式设置
	Dog_Init->Dog_Mode = Dog_Remote_Control_Mode;
	Dog_Init->Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode == Stop_M;
	Dog_Init->Dog_Control_Data.Dog_Set_Move_Data.Dog_Sport_Mode == Trot_M;
	
	Dog_Init->Dog_Control_Data.Dog_Order_Mode_Data = Get_Order_Mode_Address();
	Dog_Order_Mode_Init();
	Dog_Init->Dog_Control_Data.Dog_Remote_Mode = Dog_RM_Att_Control_Mode;
	Dog_Init->Dog_Control_Data.Dog_Set_Move_Data.All_Leg_Move_Time = 1000;
	Dog_Init->Dog_Control_Data.Dog_Set_Move_Data.Leg_Move_Duty = 0.5;
	Dog_Init->Dog_Control_Data.Dog_Set_Move_Data.Single_Leg_Move_Time = 500;



	//设置机器狗基本配置
	Dog_Init->Dog_Config_Data.servo_len = Lite_Dog_Len;
	Dog_Init->Dog_Config_Data.servo_angle_range = 360.f;
	Dog_Init->Dog_Config_Data.body_length = 157.0f;
	Dog_Init->Dog_Config_Data.body_width = 108.0f;
	Dog_Init->Dog_Config_Data.foot_width = 116.0f;
	Dog_Init->Dog_Config_Data.thigh_length = 54.4117f;
	Dog_Init->Dog_Config_Data.calf_length = 58.6911f;
	Dog_Init->Dog_Config_Data.joint_offset = 310;

	Dog_Init->Dog_Config_Data.pitch_error = Dog_Pit_Error;
	Dog_Init->Dog_Config_Data.roll_error = Dog_Roll_Error;
	Dog_Init->Dog_Config_Data.yaw_error = Dog_Yaw_Error;

	Dog_Init->Dog_Config_Data.pitch_max = Dog_Pit_Max;
	Dog_Init->Dog_Config_Data.roll_max = Dog_Roll_Max;
	Dog_Init->Dog_Config_Data.yaw_max = Dog_Yaw_Max;

	Dog_Init->Dog_Config_Data.x_max = 20.00f;
	Dog_Init->Dog_Config_Data.x_min = -20.00f;
	Dog_Init->Dog_Config_Data.y_max = 20.00f;
	Dog_Init->Dog_Config_Data.y_min = -20.00f;
	Dog_Init->Dog_Config_Data.z_max = 90.00f;
	Dog_Init->Dog_Config_Data.z_min = 50.00f;

	Dog_Init->Dog_State_Data.Dog_Mpu_Data = Return_Mpu_Address(); 
	Dog_Init->Dog_State_Data.Dog_Body_Now_Att = Body_All_Up;

	Dog_Init->Target_Attitude.x = Dog_Init_Gravity;
	Dog_Init->Target_Attitude.y = Dog_Init_Gravity;
	Dog_Init->Target_Attitude.z = Dog_Init_Height;

	//保存控制值地址
	Dog_Init->Dog_State_Data.Dog_Body_Set_Cood[0] = Dog_Init->Dog_LF_Leg_Data.Set_Leg_Loc;
	Dog_Init->Dog_State_Data.Dog_Body_Set_Cood[1] = Dog_Init->Dog_RF_Leg_Data.Set_Leg_Loc;
	Dog_Init->Dog_State_Data.Dog_Body_Set_Cood[2] = Dog_Init->Dog_LB_Leg_Data.Set_Leg_Loc;
	Dog_Init->Dog_State_Data.Dog_Body_Set_Cood[3] = Dog_Init->Dog_RB_Leg_Data.Set_Leg_Loc;

	//保存当前值地址
	Dog_Init->Dog_State_Data.Dog_Body_Now_Cood[0] = Dog_Init->Dog_LF_Leg_Data.Now_Leg_Loc;
	Dog_Init->Dog_State_Data.Dog_Body_Now_Cood[1] = Dog_Init->Dog_RF_Leg_Data.Now_Leg_Loc;
	Dog_Init->Dog_State_Data.Dog_Body_Now_Cood[2] = Dog_Init->Dog_LB_Leg_Data.Now_Leg_Loc;
	Dog_Init->Dog_State_Data.Dog_Body_Now_Cood[3] = Dog_Init->Dog_RB_Leg_Data.Now_Leg_Loc;

	Dog_Init->Dog_Control_Data.Dog_Remote_Control = Return_Rocker_Address();
	Dog_Init->Dog_Control_Data.Dog_Set_Move_Data.Dog_Config_Data = &Dog_Init->Dog_Config_Data;

	Dog_Init->Dog_LF_Leg_Data.Leg_Loc = Dog_LF_Leg;
	Dog_Init->Dog_RF_Leg_Data.Leg_Loc = Dog_RF_Leg;
	Dog_Init->Dog_LB_Leg_Data.Leg_Loc = Dog_LB_Leg;
	Dog_Init->Dog_RB_Leg_Data.Leg_Loc = Dog_RB_Leg;
	
	Dog_Init->Dog_LF_Leg_Data.Leg_Motor_Message[0] = Return_Servo_Address(1);
	Dog_Init->Dog_RF_Leg_Data.Leg_Motor_Message[0] = Return_Servo_Address(2);
	Dog_Init->Dog_LB_Leg_Data.Leg_Motor_Message[0] = Return_Servo_Address(3);
	Dog_Init->Dog_RB_Leg_Data.Leg_Motor_Message[0] = Return_Servo_Address(4);

	Dog_Init->Dog_LF_Leg_Data.Leg_Motor_Message[1] = Return_Servo_Address(5);
	Dog_Init->Dog_RF_Leg_Data.Leg_Motor_Message[1] = Return_Servo_Address(6);
	Dog_Init->Dog_LB_Leg_Data.Leg_Motor_Message[1] = Return_Servo_Address(7);
	Dog_Init->Dog_RB_Leg_Data.Leg_Motor_Message[1] = Return_Servo_Address(8);

	Dog_Init->Dog_LF_Leg_Data.Leg_Motor_Message[2] = Return_Servo_Address(9);
	Dog_Init->Dog_RF_Leg_Data.Leg_Motor_Message[2] = Return_Servo_Address(10);
	Dog_Init->Dog_LB_Leg_Data.Leg_Motor_Message[2] = Return_Servo_Address(11);
	Dog_Init->Dog_RB_Leg_Data.Leg_Motor_Message[2] = Return_Servo_Address(12);

	Lite_Dog_Read_Flash_Data(Dog_Init);
}


Lite_Dog_Mode_Enum Dog_Mode_Last;//保存上一次模式值
void Lite_Dog_Task(void)
{

	Lite_Dog_Data_Init(&Lite_Dog);
	while(1)
	{	
		// 状态更新
		Lite_Dog_Attitude_Update(&Lite_Dog.Dog_State_Data);

		// 倒地自恢复(可注释)
		// Dog_Recovery_Event(&Lite_Dog);

		// 获取舵机参数(可注释)
		// Lite_Dog_Servo_To_Data(&Lite_Dog);

		//根据模式修改足端坐标
		//初始化模式
		if(Lite_Dog.Dog_Mode == Dog_Init_Mode)
		{
			Lite_Dog_Init_Motion_Set(&Lite_Dog);
			if(Lite_Dog.Dog_State == Dog_Done)
			{
				Lite_Dog.Dog_Mode = Dog_Order_Control_Mode;
			}
		}
		//遥控器模式
		if(Lite_Dog.Dog_Mode == Dog_Remote_Control_Mode)
		{
			// 遥控器模式设置
			Dog_Remote_Control_Mode_Set(&Lite_Dog.Dog_Control_Data);
			// 遥控器模式更新
			Dog_Remote_Control_Mode_Update(&Lite_Dog);
		}
		//指令模式
		else if(Lite_Dog.Dog_Mode == Dog_Order_Control_Mode)
		{
			if(Dog_Mode_Last != Dog_Order_Control_Mode)
			{
				// 如果之前不是指令模式，初始化指令模式相关数据值
				Dog_Order_Mode_Init();
			}
			// 指令模式更新
			Dog_Order_Motion_Data_Update(&Lite_Dog);
		} 

		// 判断是否是处于可以控制状态
		if(Lite_Dog.Dog_State == Dog_OK)
		{
			// 坐标解算，坐标-角度
			Lite_Dog_Coordinate_To_Angle(&Lite_Dog,Lite_Dog.Dog_Set_XY_Coordinate[0],Lite_Dog.Dog_Set_XY_Coordinate[1],Lite_Dog.Dog_Set_XY_Coordinate[2]);

			// 舵机值发送,角度-舵机值
			Lite_Dog_Data_To_Servo(&Lite_Dog,0,0);
		}	
	
		Dog_Mode_Last = Lite_Dog.Dog_Mode;
		LOS_TaskDelay(Dog_Delay_Time_Set);
	}
}




void Dog_Test_Usb_Callback(char *type,char* Leg)
{
	if(memcmp(type, "Dog-Leg", 7) == 0)
	{
		if(memcmp(Leg, "LF", 2) == 0)
		{
			Usb_Write_Data("Dog_LR_Angle_Data: A: %f B: %f C: %f \r\n",\
			Lite_Dog.Dog_LF_Leg_Data.Leg_Angle_Now[0],Lite_Dog.Dog_LF_Leg_Data.Leg_Angle_Now[1],Lite_Dog.Dog_LF_Leg_Data.Leg_Angle_Now[2]);
		}
		else if(memcmp(Leg, "RF", 2) == 0)
		{
			Usb_Write_Data("Dog_RF_Angle_Data: A: %f B: %f C: %f \r\n",\
			Lite_Dog.Dog_RF_Leg_Data.Leg_Angle_Now[0],Lite_Dog.Dog_RF_Leg_Data.Leg_Angle_Now[1],Lite_Dog.Dog_RF_Leg_Data.Leg_Angle_Now[2]);
		}
		else if(memcmp(Leg, "LB", 2) == 0)
		{
			Usb_Write_Data("Dog_LB_Angle_Data: A: %f B: %f C: %f \r\n",\
			Lite_Dog.Dog_LB_Leg_Data.Leg_Angle_Now[0],Lite_Dog.Dog_LB_Leg_Data.Leg_Angle_Now[1],Lite_Dog.Dog_LB_Leg_Data.Leg_Angle_Now[2]);
		}
		else if(memcmp(Leg, "RB", 2) == 0)
		{
			Usb_Write_Data("Dog_RB_Angle_Data: A: %f B: %f C: %f \r\n",\
			Lite_Dog.Dog_RB_Leg_Data.Leg_Angle_Now[0],Lite_Dog.Dog_RB_Leg_Data.Leg_Angle_Now[1],Lite_Dog.Dog_RB_Leg_Data.Leg_Angle_Now[2]);
		}
	}
}

void Lite_Dog_Set_Init(void)
{
	for(int i = 0;i < 4;i ++)
	{
		Lite_Dog.Dog_Set_XY_Coordinate[0][i] = 0;
		Lite_Dog.Dog_Set_XY_Coordinate[1][i] = 0;
		Lite_Dog.Dog_Set_XY_Coordinate[2][i] = 80;
	}	
}

void Lite_Dog_Change_Mode(Lite_Dog_Mode_Enum Dog_Mode_Change)
{
	Lite_Dog.Dog_Mode = Dog_Mode_Change;
}

Lite_Dog_Struct* Get_Lite_Dog_Data_Address(void)
{
	return &Lite_Dog;
}

